function poseSmoothing( tracksdir, outdir, method, skipframes )
%poseSmoothing - Smooths (and interpolated if needed) the pose tracks.

% Examples:
%   tracksdir = ['/Users/trananh/Desktop/gestures/KTH/ground-truth/' ...
%     'walking/person01_walking_d1_uncomp/00001-00076/skeleton-tracks/'];
%   outdir = ['/Users/trananh/Desktop/gestures/KTH/ground-truth/' ...
%     'walking/person01_walking_d1_uncomp/00001-00076/skeleton-tracks/'];
  
  % Set default smoothing method to rloess
  if ~exist('method', 'var')
    method = 'rloess';
  end
  
  % Set default skip variable to 10 frames
  if ~exist('skipframes', 'var')
    skipframes = 10;
  end
  
  if ~strcmp(outdir(end), '/')
    outdir = [outdir '/'];
  end
  if ~strcmp(tracksdir(end), '/')
    tracksdir = [tracksdir '/'];
  end
  
  % Create dir for output if necessary
  if ~exist(outdir, 'dir')
    mkdir(outdir);
  end
  
  prefix = '';
  
  % Retrieve all tracks
  fprintf('Smoothing dir: %s\n', tracksdir);
  fileslist = dir([tracksdir '*.txt']);
  
  % For each track, load and smooth the pose
  for i = 1:numel(fileslist)
    file = fileslist(i).name;
    fprintf(' - file %s\n', file);
    
    % Import data
    track = importdata([tracksdir file]);
    
    % Smooth pose data
    smoothed = smoothpose(track.data, method, skipframes);
    
    % Write back smoothed data
    outfile = [outdir prefix file];
    fid = fopen(outfile, 'w');
    heading = track.colheaders;
    for j=1:numel(heading)
      if j < numel(heading)
        fprintf(fid, '%s,', heading{j});
      else
        fprintf(fid, '%s\n', heading{j});
      end
    end
    fclose(fid);
    dlmwrite(outfile, smoothed, 'delimiter', ',', 'precision', '%.4f', ...
      '-append');
  end
  
end


function smoothed = smoothpose(data, method, skipframes)
%smoothpose - Smooths the pose track.
  
  % Smoothing parameter (default to 30%)
  span = 0.3;
  
  % Break data into slices if the difference between successive frames
  % is greater than skipframes.  That way we won't smooth across
  % intervals when the actor is out of frame.
  slices = {};
  frames = data(:,1);
  dframes = diff(frames);
  breakpoints = find(dframes > skipframes);
  if numel(breakpoints) == 0
    slices = { data };
  else
    fprintf('   **** found breakpoints: [%s]\n', mat2str(breakpoints));
    start = 1;
    for i = 1:numel(breakpoints)
      slices = cat(1, slices, {data(start:breakpoints(i),:)});
      start = breakpoints(i)+1;
    end
    assert(start <= size(data,1));  % due to nature of diff
    slices = cat(1, slices, {data(start:end,:)});
  end
  
  % Smooth across each slice
  smoothed = [];
  for i = 1:numel(slices)
    slice = slices{i};
    if size(slice,1) > 1
      x = slice(:,1) - min(slice(:,1)) + 1;
      for c = 2:size(slice,2)
        slice(:,c) = smooth(x,slice(:,c),span,method);
      end
    end
    smoothed = cat(1,smoothed,slice);
  end
  assert(isequal(size(smoothed), size(data)));
  
end
